#ifndef MONGOTYPES_H
#define MONGOTYPES_H

#include <iostream>
#include <vector>
#include <map>
#include <utility>
#include <cxxabi.h>
#include <opencv2/core/core.hpp>
#include "MongoIO.h"
#include "KeyFrame.h"

namespace ORB_SLAM2 {

namespace MongoIO{

namespace types {

//! list of types
template <typename Tp> class b_vector;
template <typename Tp> class b_list;
template <typename Tp> class b_set;
template <typename Tk, typename Tv> class b_map;

typedef b_vector<float> b_vector_float;
typedef b_vector<unsigned int> b_vector_uint;
typedef b_vector<std::vector<unsigned int> > b_vvector_uint;
typedef b_vector<cv::KeyPoint> b_vector_keypoint;
typedef b_vector<size_t> b_vector_sizet;
typedef b_vector<std::vector<size_t> > b_vvector_sizet;
typedef b_vector<std::vector<std::vector<size_t> > > b_grid;
typedef b_map<uint32_t , double> b_bowvec;
typedef b_map<uint32_t , std::vector<unsigned int> > b_bowfeat;

//! class b_keypoint
class b_keypoint {
public:
    b_keypoint(const cv::KeyPoint &value);

    b_keypoint(const std::string &key, const cv::KeyPoint &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator() (bsoncxx::builder::stream::key_context<> ac) const;

    static cv::KeyPoint from(const bsoncxx::document::element &elem);

    static cv::KeyPoint from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const cv::KeyPoint value_;
};

//! class b_mat
class b_mat {
public:
    b_mat(const cv::Mat &value);

    b_mat(const std::string &key, const cv::Mat &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator()(bsoncxx::builder::stream::key_context<> kc) const;

    static cv::Mat from(const bsoncxx::document::element &elem);

    static cv::Mat from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const cv::Mat value_;
};

//! class b_vector
template <typename Tp>
class b_vector {
public:
    b_vector(const std::vector<Tp> &value);

    b_vector(const std::string &key, const std::vector<Tp> &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator()(bsoncxx::builder::stream::key_context<> kc) const;

    static std::vector<Tp> from(const bsoncxx::document::element &elem);

    static std::vector<Tp> from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const std::vector<Tp> value_;
};

//! class b_set
template <typename Tp>
class b_set {
public:
    b_set(const std::set<Tp> &value);

    b_set(const std::string &key, const std::set<Tp> &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator()(bsoncxx::builder::stream::key_context<> kc) const;

    static std::set<Tp> from(const bsoncxx::document::element &elem);

    static std::set<Tp> from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const std::set<Tp> value_;
};

//! class b_list
template <typename Tp>
class b_list {
public:
    b_list(const std::list<Tp> &value);

    b_list(const std::string &key, const std::list<Tp> &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator()(bsoncxx::builder::stream::key_context<> kc) const;

    static std::list<Tp> from(const bsoncxx::document::element &elem);

    static std::list<Tp> from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const std::list<Tp> value_;
};

//! class b_map
template <typename Tk, typename Tv>
class b_map{
public:
    b_map(const std::map<Tk, Tv> &value);

    b_map(const std::string &key, const std::map<Tk, Tv> &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator()(bsoncxx::builder::stream::key_context<> kc) const;

    static std::map<Tk, Tv> from(const bsoncxx::document::element &elem);

    static std::map<Tk, Tv> from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const std::map<Tk, Tv> value_;
};

//! class b_pair
template <typename Tk, typename Tv>
class b_pair{
public:
    b_pair(const std::pair<Tk, Tv> &value);

    b_pair(const std::string &key, const std::pair<Tk, Tv> &value);

    void operator()(bsoncxx::builder::stream::array_context<> ac) const;

    void operator()(bsoncxx::builder::stream::key_context<> kc) const;

    static std::pair<Tk, Tv> from(const bsoncxx::document::element &elem);

    static std::pair<Tk, Tv> from(const bsoncxx::array::element &elem);

public:
    const std::string key_;

    const std::pair<Tk, Tv> value_;
};

/*
 *  value function defination
 */
//! get value for write
inline bool get_value(const bool& value) { return value; }

inline int32_t get_value(const int8_t& value) { return static_cast<int32_t>(value); }

inline int32_t get_value(const uint8_t& value) { return static_cast<int32_t>(value); }

inline int32_t get_value(const int32_t& value) { return value; }

inline int32_t get_value(const uint32_t & value) { return static_cast<int32_t>(value); }

inline int64_t get_value(const int64_t& value) { return value; }

inline int64_t get_value(const uint64_t& value) { return static_cast<int64_t>(value); }

inline double get_value(const float& value) { return static_cast<double>(value); }

inline double get_value(const double& value) { return static_cast<double>(value); }

inline std::string get_value(const std::string& value) { return value; }

inline b_keypoint get_value(const cv::KeyPoint& value) { return b_keypoint(value); }

inline bsoncxx::types::b_oid get_value(const bsoncxx::types::b_oid& value) { return bsoncxx::types::b_oid(value); }

inline b_mat get_value(const cv::Mat& value) { return b_mat(value); }

template <typename Tp>
inline b_vector<Tp> get_value(const std::vector<Tp> value) { return b_vector<Tp>(value); }

template <typename Tp>
inline b_set<Tp> get_value(const std::set<Tp> value) { return b_set<Tp>(value); }

template <typename Tp>
inline b_list<Tp> get_value(const std::list<Tp> value) { return b_list<Tp>(value); }

template <typename Tk, typename Tv>
inline b_pair<Tk, Tv> get_value(const std::pair<Tk, Tv> value) { return b_pair<Tk, Tv>(value); }

//! get value for read
//! NOTE! There all define for bsoncxx::array::element, for the template std::map's and std::vector's element
//! the bsoncxx::document::element can directly use bsoncxx lib
inline void get_value(const bsoncxx::array::element &elem, bool& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_bool)
        std::cerr << "Error in get_value to bool!!! " << std::endl;
    value = elem.get_bool();
}

inline void get_value(const bsoncxx::array::element &elem, int8_t & value)
{
    if(!elem || elem.type() != bsoncxx::type::k_int32)
        std::cerr << "Error in get_value to int8_t!!! " << std::endl;
    value = static_cast<int8_t>(elem.get_int32());
}

inline void get_value(const bsoncxx::array::element &elem, uint8_t & value)
{
    if(!elem || elem.type() != bsoncxx::type::k_int32)
        std::cerr << "Error in get_value to uint8_t!!! " << std::endl;
    value = static_cast<uint8_t>(elem.get_int32());
}

inline void get_value(const bsoncxx::array::element &elem, int32_t & value)
{
    if(!elem || elem.type() != bsoncxx::type::k_int32)
        std::cerr << "Error in get_value to int32_t!!! " << std::endl;
    value = elem.get_int32();
}

inline void get_value(const bsoncxx::array::element &elem, uint32_t & value)
{
    if(!elem || elem.type() != bsoncxx::type::k_int32)
        std::cerr << "Error in get_value to uint32_t!!! " << std::endl;
    value = static_cast<uint32_t>(elem.get_int32());
}

inline void get_value(const bsoncxx::array::element &elem, int64_t & value)
{
    if(!elem || elem.type() != bsoncxx::type::k_int64)
        std::cerr << "Error in get_value to int64_t!!! " << std::endl;
    value = elem.get_int64();
}

inline void get_value(const bsoncxx::array::element &elem, uint64_t & value)
{
    if(!elem || elem.type() != bsoncxx::type::k_int64)
        std::cerr << "Error in get_value to uint64_t!!! " << std::endl;
    value = static_cast<uint64_t>(elem.get_int64());
}

inline void get_value(const bsoncxx::array::element &elem, float& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_double)
        std::cerr << "Error in get_value to float!!! " << std::endl;
    value = static_cast<float>(elem.get_double());
}

inline void get_value(const bsoncxx::array::element &elem, double& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_double)
        std::cerr << "Error in get_value to double!!! " << std::endl;
    value = elem.get_double();
}

inline void get_value(const bsoncxx::array::element &elem, std::string& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_utf8)
        std::cerr << "Error in get_value to string!!! " << std::endl;
    value = std::string(elem.get_utf8().value);
}

inline void get_value(const bsoncxx::array::element &elem, cv::KeyPoint& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in get_value to cv::KeyPoint!!! " << std::endl;
    value = b_keypoint::from(elem);
}

inline void get_value(const bsoncxx::array::element &elem, bsoncxx::types::b_oid& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_oid)
        std::cerr << "Error in get_value to bsoncxx::types::b_oid!!! " << std::endl;
    value = elem.get_oid();
}

template <typename Tp>
inline void get_value(const bsoncxx::array::element &elem, std::vector<Tp>& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in get_value to std::vector< "
                  << abi::__cxa_demangle(typeid(Tp).name(), nullptr, nullptr, nullptr) <<" >!!! " << std::endl;
    value = b_vector<Tp>::from(elem);
}

template <typename Tp>
inline void get_value(const bsoncxx::array::element &elem, std::set<Tp>& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in get_value to std::set< "
                  << abi::__cxa_demangle(typeid(Tp).name(), nullptr, nullptr, nullptr) <<" >!!! " << std::endl;
    value = b_set<Tp>::from(elem);
}

template <typename Tp>
inline void get_value(const bsoncxx::array::element &elem, std::list<Tp>& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in get_value to std::list< "
                  << abi::__cxa_demangle(typeid(Tp).name(), nullptr, nullptr, nullptr) <<" >!!! " << std::endl;
    value = b_list<Tp>::from(elem);
}

template <typename Tk, typename Tv>
inline void get_value(const bsoncxx::array::element &elem, std::pair<Tk, Tv>& value)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in get_value to std::pair< "
                  << abi::__cxa_demangle(typeid(Tk).name(), nullptr, nullptr, nullptr) << ", "
                  << abi::__cxa_demangle(typeid(Tv).name(), nullptr, nullptr, nullptr) <<" >!!! " << std::endl;
    value = b_pair<Tk, Tv>::from(elem);
}

/*
 *  type class defination
 */
//! b_vector
template <typename Tp>
b_vector<Tp>::b_vector(const std::vector<Tp> &value) : value_(value) {}

template <typename Tp>
b_vector<Tp>::b_vector(const std::string &key, const std::vector<Tp> &value) : key_(key), value_(value) {}

template <typename Tp>
void b_vector<Tp>::operator()(bsoncxx::builder::stream::array_context<> ac) const {
    ac << open_array;
    std::for_each(value_.begin(), value_.end(), [&](Tp val){ ac << get_value(val); });
    ac << close_array;
}

template <typename Tp>
void b_vector<Tp>::operator()(bsoncxx::builder::stream::key_context<> kc) const {
    bsoncxx::builder::stream::array array{};
    std::for_each(value_.begin(), value_.end(), [&](Tp val) { array << get_value(val); });
    kc << key_ << array.view();
}

template <typename Tp>
std::vector<Tp> b_vector<Tp>::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_vector!!! input document::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    const int N = std::distance(std::begin(subarray), std::end(subarray));

    std::vector<Tp> vect(N);

    for(int i = 0; i < N; i++) {
        get_value(subarray[i], vect[i]);
    }

    return vect;
}

template <typename Tp>
std::vector<Tp> b_vector<Tp>::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_vector!!! input document::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    const int N = std::distance(std::begin(subarray), std::end(subarray));

    std::vector<Tp> vect(N);

    for(int i = 0; i < N; i++) {
        get_value(subarray[i], vect[i]);
    }

    return vect;
}

//! b_set
template <typename Tp>
b_set<Tp>::b_set(const std::set<Tp> &value) : value_(value) {}

template <typename Tp>
b_set<Tp>::b_set(const std::string &key, const std::set<Tp> &value) : key_(key), value_(value) {}

template <typename Tp>
void b_set<Tp>::operator()(bsoncxx::builder::stream::array_context<> ac) const {
    ac << open_array;
    std::for_each(value_.begin(), value_.end(), [&](Tp val){ ac << get_value(val); });
    ac << close_array;
}

template <typename Tp>
void b_set<Tp>::operator()(bsoncxx::builder::stream::key_context<> kc) const {
    bsoncxx::builder::stream::array array{};
    std::for_each(value_.begin(), value_.end(), [&](Tp val) { array << get_value(val); });
    kc << key_ << array.view();
}

template <typename Tp>
std::set<Tp> b_set<Tp>::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_vector!!! input document::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    const int N = std::distance(std::begin(subarray), std::end(subarray));

    std::set<Tp> set;

    for(int i = 0; i < N; i++) {
        Tp val;
        get_value(subarray[i], val);
        set.insert(val);
    }

    return set;
}

template <typename Tp>
std::set<Tp> b_set<Tp>::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_vector!!! input array::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    const int N = std::distance(std::begin(subarray), std::end(subarray));

    std::set<Tp> set;

    for(int i = 0; i < N; i++) {
        Tp val;
        get_value(subarray[i], val);
        set.insert(val);
    }

    return set;
}


//! b_list
template <typename Tp>
b_list<Tp>::b_list(const std::list<Tp> &value) : value_(value) {}

template <typename Tp>
b_list<Tp>::b_list(const std::string &key, const std::list<Tp> &value) : key_(key), value_(value) {}

template <typename Tp>
void b_list<Tp>::operator()(bsoncxx::builder::stream::array_context<> ac) const {
    ac << open_array;
    std::for_each(value_.begin(), value_.end(), [&](Tp val){ ac << get_value(val); });
    ac << close_array;
}

template <typename Tp>
void b_list<Tp>::operator()(bsoncxx::builder::stream::key_context<> kc) const {
    bsoncxx::builder::stream::array array{};
    std::for_each(value_.begin(), value_.end(), [&](Tp val) { array << get_value(val); });
    kc << key_ << array.view();
}

template <typename Tp>
std::list<Tp> b_list<Tp>::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_vector!!! input document::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    const int N = std::distance(std::begin(subarray), std::end(subarray));

    std::list<Tp> list;

    for(int i = 0; i < N; i++) {
        Tp val;
        get_value(subarray[i], val);
        list.push_back(val);
    }

    return list;
}

template <typename Tp>
std::list<Tp> b_list<Tp>::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_vector!!! input array::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    const int N = std::distance(std::begin(subarray), std::end(subarray));

    std::list<Tp> list;

    for(int i = 0; i < N; i++) {
        Tp val;
        get_value(subarray[i], val);
        list.push_back(val);
    }

    return list;
}

//! b_map
template <typename Tk, typename Tv>
b_map<Tk, Tv>::b_map(const std::map<Tk, Tv> &value) : value_(value) {}

template <typename Tk, typename Tv>
b_map<Tk, Tv>::b_map(const std::string &key, const std::map<Tk, Tv> &value) : key_(key), value_(value) {}

template <typename Tk, typename Tv>
void b_map<Tk, Tv>::operator()(bsoncxx::builder::stream::array_context<> ac) const {
    ac << open_array;
    std::for_each(value_.begin(), value_.end(), [&](std::pair<Tk, Tv> val){
      ac << open_array << get_value(val.first) << get_value(val.second) << close_array;
    });
    ac << close_array;
}

template <typename Tk, typename Tv>
void b_map<Tk, Tv>::operator()(bsoncxx::builder::stream::key_context<> kc) const {
    bsoncxx::builder::stream::array array{};
    std::for_each(value_.begin(), value_.end(), [&](std::pair<Tk, Tv> val){
      array << open_array << get_value(val.first) << get_value(val.second) << close_array;
    });
    kc << key_  << array.view();
}

template <typename Tk, typename Tv>
inline std::map<Tk, Tv> b_map<Tk, Tv>::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_map!!! input document::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    std::map<Tk, Tv> map;
    std::for_each(subarray.begin(), subarray.end(), [&](const bsoncxx::array::element arr){
        Tk key; Tv value;
        get_value(arr[0], key);
        get_value(arr[1], value);
        map.insert(std::pair<Tk, Tv>(key, value));
    });

    return map;
}

template <typename Tk, typename Tv>
std::map<Tk, Tv> b_map<Tk, Tv>::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_map!!! input array::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    std::map<Tk, Tv> map;
    std::for_each(subarray.begin(), subarray.end(), [&](const bsoncxx::array::element arr){
      Tk key; Tv value;
      get_value(arr[0], key);
      get_value(arr[1], value);
      map.insert(std::pair<Tk, Tv>(key, value));
    });

    return map;
}

//! b_pair
template <typename Tk, typename Tv>
b_pair<Tk, Tv>::b_pair(const std::pair<Tk, Tv> &value) : value_(value) {}

template <typename Tk, typename Tv>
b_pair<Tk, Tv>::b_pair(const std::string &key, const std::pair<Tk, Tv> &value) : key_(key), value_(value) {}

template <typename Tk, typename Tv>
void b_pair<Tk, Tv>::operator()(bsoncxx::builder::stream::array_context<> ac) const {
    ac << open_array << get_value(value_.first) << get_value(value_.second) << close_array;
}

template <typename Tk, typename Tv>
void b_pair<Tk, Tv>::operator()(bsoncxx::builder::stream::key_context<> kc) const {
    kc << key_ << open_array << get_value(value_.first) << get_value(value_.second) << close_array;
}

template <typename Tk, typename Tv>
inline std::pair<Tk, Tv> b_pair<Tk, Tv>::from(const bsoncxx::document::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_pair!!! input document::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    Tk key; Tv value;
    get_value(subarray[0], key);
    get_value(subarray[1], value);

    return std::pair<Tk, Tv>(key, value);
}

template <typename Tk, typename Tv>
std::pair<Tk, Tv> b_pair<Tk, Tv>::from(const bsoncxx::array::element &elem)
{
    if(!elem || elem.type() != bsoncxx::type::k_array)
        std::cerr << "Error in b_pair!!! input array::element is not correct" << std::endl;

    bsoncxx::array::view subarray{elem.get_array().value};

    Tk key; Tv value;
    get_value(subarray[0], key);
    get_value(subarray[1], value);

    return std::pair<Tk, Tv>(key, value);
}

}//! namespace types

}//! namespace MongoIO

}//! namespace ORB_SLAM2

#endif //MONGOTYPES_H
